#include "dyn_controlChaos.h"

ControlChaos::ControlChaos(MultiMaps *_map,
                           mat orbitPoints,
                           colvec initCond,
                           mapsGetPtr getPtr,
                           mapsSetPtr setPtr,
                           double deltaMax,
                           double epsilonNeighborhood,
                           int controlIterations,
                           int maxIterations)
    : map(_map)
{
    getParameter = getPtr;
    setParameter = setPtr;

    setOrbitPoints(orbitPoints);
    setInitialCondition(initCond);
    setDeltaMax(deltaMax);
    setEpsilonNeighborhood(epsilonNeighborhood);
    setControlIterations(controlIterations);
    setMaxIterations(maxIterations);
}

mat ControlChaos::control()
{
    // We prepare a matrix that holds
    // the results of the control algorithm.
    mat results(maxIterations,init.n_rows);
    results.row(0) = init.t();

    // We set the size of the column vector
    // containing the parameter variation values.
    delta.set_size(controlIterations);

    // We fetch the current parameter of the map
    // and set it to our default parameter.
    setDefaultParameter(CALL_MEMBER_FN(map,getParameter)());

    // We set the number of iterates to 0.
    int controlledIterates(0);

    // We iterate the map until we find
    // a point near an orbit point.
    for(int k=1;k<maxIterations;k++)
    {
        results.row(k) = map->iterate(results.row(k-1).t(),1).t();

        if (controlledIterates>controlIterations)
            continue;

        for (unsigned int i=0;i<orbitPoints.n_rows;i++)
        {
            rowvec diff = results.row(k) - orbitPoints.row(i);

            CALL_MEMBER_FN(map,setParameter)(defaultParameter);

            if (norm(diff,2) < epsilonNeighborhood)
            {
                if (controlledIterates==0)
                {
                    setIteratesToControl(k);
                }

                double deltaParameter = computeDelta(i, diff);
                delta(controlledIterates) = deltaParameter;

                if (fabs(deltaParameter) > deltaMax)
                {
                    deltaParameter = deltaMax*tanh(deltaParameter/deltaMax);
                }

                CALL_MEMBER_FN(map,setParameter)(defaultParameter+deltaParameter);

                controlledIterates++;
                break;
            }
        }
    }
    return results;
}
